module CamReaderCW(
	iCLK,
	iPCLK,
	iData,
	iX,
	iY,
	iFrame,
	oDebug,
	oStatus,
	iRST,
	//Acos side
	oTrigStart,
	oTrigFunc,
	iTrigBusy,
	iTrigError,
	oTrigData,
	iTrigResult,
	oTXD,
	iTXStart
);

input iCLK;
input iPCLK;
input[7:0] iData;
input[9:0] iX, iY;
input iFrame;
output [401:0] oDebug;
assign oDebug = {CoordY, 6'd0,CoordX, fifo_in, fifo_out, Add1Res, Add1B, Add1A, IntToFP1Res, IntToFP1A, SinglePrecFloatSqrtRes, SinglePrecFloatSqrtA, DivRes, DivB, cnt};
output [17:0] oStatus;
assign oStatus = {5'd0, iFrame, fifo_full, fifo_empty, fifo_wr, fifo_rd, state_read, state, busy, start};
input iRST;
//External acos
output reg oTrigStart;
output reg [1:0] oTrigFunc = 2'b10;
input iTrigBusy, iTrigError;
output reg [31:0] oTrigData;
input [31:0] iTrigResult;
input iTXStart;
output oTXD;

localparam CENTERX = 320;
localparam CENTERY = 240;
localparam RADIUS  = 170;
localparam RADIUSF = 32'h432A0000;
localparam PI2		 = 32'h40c90fdb;	// 2*Pi

reg[1:0] frame;
always@(posedge iFrame or posedge iRST)begin
	if(iRST)
		frame <= 'd0;
	else if(frame < 2'b11) frame <= frame + 1'd1;
end

reg[1:0] state_read;
reg[9:0] x, y;
reg[7:0] data;
always@ (posedge iPCLK or posedge iRST) begin	
	if(iRST) begin
		state_read <= 'b0;
		fifo_rd <= 'b0;
		fifo_wr <= 'b0;
		start <= 'b0;
	end
	else begin
		if((((CENTERX - iX)*(CENTERX - iX) + (CENTERY - iY)*(CENTERY - iY)) > ((RADIUS - 2) * (RADIUS - 2))) &&
			(((CENTERX - iX)*(CENTERX - iX) + (CENTERY - iY)*(CENTERY - iY)) < ((RADIUS + 2) * (RADIUS + 2))) && (frame == 'd2)) 
		begin
			fifo_wr <= 'b1;
			fifo_in <= {iX, iY, iData};
		end
		else fifo_wr <= 'b0;
	
		case(state_read)
			0: begin		// read fifo if not empty
				if((~fifo_empty) || (fifo_usedw > 'd0))begin
					fifo_rd <= 'b1;
					state_read <= 'b1;
					start <= 'b0;
				end
			end		
			1: begin		// wait for data 
				fifo_rd <= 'b0;
				{x, y, data} <= fifo_out;
				start <= ~start;
				if(busy)
					state_read <= 'd2;
			end		
			2: begin
				start <= 'b0;
				if(!busy) state_read <= 'b0;
			end
		endcase
	end
end

reg start, pstart, busy;
reg[3:0] state; 
reg[4:0] timer;
reg[31:0] temp;
reg[9:0] CoordX, CoordY;
reg[7:0] CamData;
reg[31:0] cnt;
always@(posedge iCLK or posedge iRST) begin
	if(iRST) begin
		busy <= 'd0;
		state <= 'd0;
		pstart <= 'd1;
		cw_wr <= 'd0;
		cnt <= 'd0;
	end
	else begin
		pstart <= start;
		if(!busy) begin			
			if({pstart, start} == 2'b01) begin
				CoordX <= x;
				CoordY <= y;
				CamData <= data;
				busy <= 1'b1;
				state <= 4'd0;								
				timer <= 5'd0;
				cw_wr <= 'd1;
			end
		end
		else begin
			case(state)
				0: begin					
					IntToFP1A <= (CENTERX - CoordX)*(CENTERX - CoordX) + (CENTERY - CoordY)*(CENTERY - CoordY);
					state <= 4'd1;	
				end
				
				1: begin
					if(timer == 5'd6)begin
						IntToFP1A <= (CENTERY - CoordY);
						SinglePrecFloatSqrtA <= IntToFP1Res;
						timer <= 5'd0;
						state <= 4'd3;
					end
					else timer <= timer + 5'b1;
				end			
				
				3:begin
					if(timer == 5'd16)begin
						DivA <= IntToFP1Res;
						DivB <= SinglePrecFloatSqrtRes;
						timer <= 5'd0;
						state <= 4'd2;
					end
					else timer <= timer + 5'b1;					
				end
				
				2:begin					
					if(timer == 5'd6) begin
						timer <= 5'd0;
						state <=  4'd6;
						oTrigData <= DivRes;
						oTrigStart <= 1'b1;
					end
					else timer <= timer + 5'b1;			
				end
				
				6: 
					if(iTrigBusy == 1'b1) state <=  4'd4;
					else if(iTrigError == 1'b1)state <= 4'd8;		///ERROR
					
				4:begin
					oTrigStart <= 'b0;
					if(iTrigError == 1'b1)state <= 'd8;		///ERROR
					else if(iTrigBusy == 'b0) begin
						if(CoordX < CENTERX + 1) begin
							temp <= iTrigResult;
							state <= 'd7;
						end
						else begin
							Add1A <= PI2;
							Add1B <= {~iTrigResult[31],iTrigResult[30:0]};
							state <=  'd5;
						end
					end
				end
				
				5:begin									
					if(timer == 5'd7) begin
						timer <= 5'd0;
						state <= 'd7;
						temp <= Add1Res;
					end
					else timer <= timer + 5'b1;
				end
				7:begin
					state <= 'd15;
					MultA <= temp;
					MultB <= RADIUSF;
				end
				15: begin									
					if(timer == 5'd5) begin
						timer <= 5'd0;
						state <=  'd14;
						FPToIntA <= MultRes;
						cw_wr <= 'd0;
					end
					else timer <= timer + 5'b1;
				end
				14: begin									
					if(timer == 5'd6) begin
						timer <= 5'd0;
						state <=  'd12;
						cw_address <= FPToIntRes[10:0];
						cw_data <= CamData;
						//cw_wr <= 'd1;
					end
					else timer <= timer + 5'b1;
				end
				12: begin
					cw_wr <= 'd1;
					if(busy)busy <= 'd0;
					cnt <= cnt + 1;
				end
				
				8: begin
					/// handle acos error
				end
				default: begin
				end
			endcase
		end
	end
end

UART_Controller uart(
	//Host side
	.iCLK(iCLK),									//50 MHz
	.iStartTransmit(iTXStart),					//Start sending
	.iStartAddress(0),						//Start address
	.iEndAddress(2047),						//End address
	.oBusy(),								//UART controller busy
	//UART transmitter side
	.oTXD(oTXD),
	//MEmory side
	.oMEM_ADDR(cw_address_rd),
	.iMEM_DATA(cw_q),
	.oMEM_CLK()
);

reg[27:0] fifo_in;
wire[27:0] fifo_out;
wire fifo_empty, fifo_full;
wire[7:0] fifo_usedw;
reg fifo_wr, fifo_rd;
FIFO fifo(
		.clock(~iPCLK),
		.data(fifo_in),
		.rdreq(fifo_rd),
		.sclr(iRST),
		.wrreq(fifo_wr),
		.empty(fifo_empty),
		.full(fifo_full),
		.q(fifo_out),
		.usedw(fifo_usedw)
);

reg[7:0] cw_data;
wire[7:0] cw_q;
reg[10:0] cw_address;
wire [17:0] cw_address_rd;
reg cw_wr;
LineBuffer lb(
	.clock(~iCLK),
	.data(cw_data),
	.rdaddress(cw_address_rd[10:0]),
	.wraddress(cw_address),
	.wren(cw_wr),
	.q(cw_q));

//6T
reg [31:0] IntToFP1A;
wire [31:0] IntToFP1Res;
IntToSinglePrecFloatConvert i2fp(
	.clock(~iCLK),
	.dataa(IntToFP1A),
	.result(IntToFP1Res)
);

//16T
reg [31:0] SinglePrecFloatSqrtA;
wire [31:0] SinglePrecFloatSqrtRes;
SinglePrecFloatSqrt sqrt(
	.clock(~iCLK),
	.data(SinglePrecFloatSqrtA),
	.result(SinglePrecFloatSqrtRes)
);

//6T
reg[31:0] DivA, DivB;
wire[31:0] DivRes;
SinglePrecFloatDiv div(
	.dataa(DivA),
	.datab(DivB),
	.result(DivRes),
	.clock(~iCLK)
);

//7T
reg[31:0] Add1A, Add1B;
wire[31:0] Add1Res;
SinglePrecFloatAdd add(
	.clock(~iCLK),
	.dataa(Add1A),
	.datab(Add1B),
	.result(Add1Res)
);

//5T
reg[31:0] MultA, MultB;
wire[31:0] MultRes;
SinglePrecFloatMult mult(
	.dataa(MultA),
	.datab(MultB),
	.result(MultRes),
	.clock(~iCLK)
);

//6T
reg [31:0] FPToIntA;
wire [31:0] FPToIntRes;
SinglePrecFloatToInt fp2i(
	.clock(~iCLK),
	.dataa(FPToIntA),
	.result(FPToIntRes)
);

endmodule
